#include "ControllerThread.h"
#include "Motor.h"

ControllerThread::ControllerThread(Motor* main, Motor* jib, Motor* portRudder, Motor* stbdRudder){
    _main = main;
    _jib = jib;
    _portRudder = portRudder;
    _stbdRudder = stbdRudder;
}

void ControllerThread::run(){
    digitalWrite(40, HIGH);
    //Serial.println("c");

    byte buffer[17];
    size_t checksum = 0;

    //THREAD_BEGIN()
    if(Serial3.available() > 0){
        //Serial.println("Serial3 Available");
        byte commandByte = Serial3.read();

        if(commandByte == MOTOR_COMMAND){

            checksum = 0;
            for(int i=0; i<8; i++){
                buffer[i] = static_cast<byte>(Serial3.read());
                checksum += buffer[i];
            }
            buffer[8] = Serial3.read();
            buffer[9] = Serial3.read();

            size_t checksumTransmitted = static_cast<size_t>(buffer[8]) + (static_cast<size_t>(buffer[9]) << 8);
            if(checksum == checksumTransmitted){
                digitalWrite(12, HIGH);

                uint16_t mainCommanded = static_cast<uint16_t>(buffer[0]) + (static_cast<uint16_t>(buffer[1]) << 8);
                uint16_t jibCommanded = static_cast<uint16_t>(buffer[2]) + (static_cast<uint16_t>(buffer[3]) << 8);
                uint16_t portCommanded = static_cast<uint16_t>(buffer[4]) + (static_cast<uint16_t>(buffer[5]) << 8);
                uint16_t stbdCommanded = static_cast<uint16_t>(buffer[6]) + (static_cast<uint16_t>(buffer[7]) << 8);

                _main->setCommandedPosition(mainCommanded);
                _jib->setCommandedPosition(jibCommanded);
                _portRudder->setCommandedPosition(portCommanded);
                _stbdRudder->setCommandedPosition(stbdCommanded);

                digitalWrite(12, LOW);
            }
        }
        else if(commandByte == STATUS){

        }
        else if(commandByte == SET_AUTONOMOUS_MODE){
            if(enableAutonomy == 0)
                enableAutonomy = 1;
            else if(enableAutonomy == 1)
                enableAutonomy = 0;
        }
        else{
            Serial3.flush();
        }
    }
    //THREAD_END()
    _delay_ms(50);

    digitalWrite(40, LOW);
}
